#include "myInit.h"

uint8_t gRCRxData[42] 		= {0};	// 遥控接收数据
uint8_t gGyroRxData[42] 	= {0};	// 陀螺仪接收数据
uint8_t gComputer_Data[105] = {0};  // 小电脑接收数据
uint8_t gPass_Data[30] 	    = {0};  // 两车通信
uint8_t gChassis_Speed_Log 	= 0; 	// 底盘速度标志位

uint8_t gCamera_Radar_Sign 	= 0; 	// 决定使用摄像头还是雷达瞄准
uint8_t gAim_Sign 			= 0; 
uint8_t gAim_Sign_Last 		= 0; 
uint8_t gPass_Sign          = 0;

float gShoot_V = 45.f;
uint8_t gLast_Button_IT = 0;

#define cHoop_X 13.15f   // 篮筐坐标 (1.[13.1,-3.46])
#define cHoop_Y -3.55f
//#define cHoop_X 13.10f   // 篮筐坐标 (2.[13.22,-3.54])
//#define cHoop_Y -3.52f 
//#define cHoop_X 13.1f  // 篮筐坐标 (3.[13.1,-3.46])
//#define cHoop_Y -3.46f
//#define cHoop_X 13.25f   // 篮筐坐标 (4.[13.25,-3.5])
//#define cHoop_Y -3.50f


union u8_i16 gJoy_RC;
union u8_f gComputer;
union u8_f gPASS;
/*---------------------*/

// 串口重新初始化
void communicateInit()
{
	MX_USART2_UART_Init();
	HAL_UARTEx_ReceiveToIdle_DMA(&huart2, gComputer_Data, 105);

	MX_USART3_UART_Init();
	HAL_UARTEx_ReceiveToIdle_DMA(&huart3, gGyroRxData, 42);

	MX_UART4_Init();
	HAL_UARTEx_ReceiveToIdle_DMA(&huart4, gRCRxData, 42);

	MX_UART7_Init();
	HAL_UARTEx_ReceiveToIdle_DMA(&huart7, gPass_Data, 30);
}

// 串口接收
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
	if (gMechInitFinishAndInFreeRTOSFlag == false)		//如果没初始化完进入FreeRTOS
	{
		if (huart == &huart4)
		{
			static uint8_t sRCLastFlagNum = 0;
			if ((Size == 14) &&
				(sRCLastFlagNum != gRCRxData[11]) &&
				(gRCRxData[0] == 0x6A) && (gRCRxData[13] == 0x7B) &&
				(My_CalCheckSum(&gRCRxData[1], 11) == gRCRxData[12]))
			{
				gMechInitMode = gRCRxData[10];
				sRCLastFlagNum = gRCRxData[11];
				
				if (gMotorONFlag == false)
				{
					if(gRCRxData[9] == 0x08 && gRCRxData[9]!=gLast_Button_IT)
						HAL_UART_Transmit_IT(&huart2,(uint8_t*)"start",5);
					if(gRCRxData[9] == 0x09 && gRCRxData[9]!=gLast_Button_IT)
						HAL_UART_Transmit_IT(&huart2,(uint8_t*)"stop",4);
					gLast_Button_IT = gRCRxData[9];
				}
			}
			HAL_UARTEx_ReceiveToIdle_DMA(&huart4, gRCRxData, 42);
		}
	} 
	/* ************************************************************************************************************************ */
	else
	{
		if (huart == &huart2)
		{
			if ((Size == 35) && (gComputer_Data[0] == 0xFF) && (gComputer_Data[1] == 0x01))
			{
				xSemaphoreGiveFromISR(UART_To_PCHandle, NULL);
			}
			HAL_UARTEx_ReceiveToIdle_DMA(&huart2, gComputer_Data, 105);
		}
		else 
		if (huart == &huart3)
		{
			if (Size == 14)
			{
				if (gGyroRxData[0] == 0X77 && (My_CalCheckSum(&gGyroRxData[1], 12) == gGyroRxData[13]))
				{
					xSemaphoreGiveFromISR(UART_To_GyroHandle, NULL);
				}
			}
			HAL_UARTEx_ReceiveToIdle_DMA(&huart3, gGyroRxData, 42);
		}
		else 
		if (huart == &huart4)
		{
			static uint8_t sRCLastFlagNum = 0;
			if ((Size == 14) &&
				(sRCLastFlagNum != gRCRxData[11]) &&
				(gRCRxData[0] == 0x6A) && (gRCRxData[13] == 0x7B) &&
				(My_CalCheckSum(&gRCRxData[1], 11) == gRCRxData[12]))
			{
				xSemaphoreGiveFromISR(UART_To_RCHandle, NULL);
				sRCLastFlagNum = gRCRxData[11];
			}
			HAL_UARTEx_ReceiveToIdle_DMA(&huart4, gRCRxData, 42);
		}
		else
		if (huart == &huart7)
		{
			if (Size == 10)
			{
				if(gPass_Data[0] == 0xAA&&gPass_Data[9] == 0xBB)
				{
					xSemaphoreGiveFromISR(UART_To_RobotHandle, NULL);
				}
				HAL_UARTEx_ReceiveToIdle_DMA(&huart4, gPass_Data, 30);
			}
		}
	}
}

/* 基础任务 ***************************************************************/

void basicsTask(void *argument)
{
	static uint8_t sRCRxData[14] 		= {0};  // 遥控接收数据
	static uint8_t sGyroRxData[14] 		= {0};	// 陀螺仪接收数据
	static uint8_t sComputerData[35]	= {0};  // 小电脑接收数据
	static uint8_t sPassData[10]	    = {0};  // 两车通信
	static uint32_t sRCLastTick 		= 0;
	static uint32_t sGyroLastTick 		= 0;
	static uint32_t sPCLastTick			= 0;
	static uint32_t sPassLastTick	    = 0;
	
	static int16_t sVx_Receive 			= 0;
	static int16_t sVy_Receive 			= 0;
	static int16_t sVx_Receive_Middle 	= 0;
	static int16_t sVy_Receive_Middle 	= 0;
	static int16_t sVz 					= 0;
	static int16_t sVx 					= 0;
	static int16_t sVy 					= 0;
	static uint8_t sLast_Button 		= 0;

	static float 	sComputer_HoopAng 		= 0;
	static float 	sCoordinates_HoopAng	= 0;
	static uint8_t 	sComputer_ErrSign 		= 0;

	static ShootCMD sShootCMD;
	static float sComputer_X 			= 0;
	static float sComputer_Y 			= 0;
	static float sHoop_Distance 		= 0;
	static float sPass_Ang      		= 0;
	static float sCamera_Distanse       = 0;
	static float sComYaw_Ang            = 0;
	static float sInlYaw_Ang            = 0;
	static uint8_t Sync_Log             = 0;
	static float sYaw_Ang 			    = 0;
	static float sYaw_Rad 		        = 0;
	static float sW 					= 0;
	static float sYaw_Aim               = 0;
	
	static float sRobot_X 				= 0;
	static float sRobot_Y 				= 0;
	static float sRobot_Distance    	= 0;
	static float sRobot_Ang    			= 0;
	static uint8_t sPC_Err      		= 0;
	static uint8_t sYaw_Err             = 0;
	static uint8_t sShoot_Yaw           = 0;
	static uint8_t sAim_Time_Camera     = 0;
	static uint8_t sAim_Time_Radar      = 0;
	static uint8_t sBlock_Sign          = 0;
	static uint8_t sRobot_Err           = 0;
	
	volatile static uint8_t sLastAttackAndDefenseMode = 0x00;
	sLastAttackAndDefenseMode = gMechInitMode;
	
	static char Pri[16]        			= {0};  // 打印的数组

	__disable_irq();
	gMechInitFinishAndInFreeRTOSFlag = true;           //串口回调的信号量开始生效
	__enable_irq();
	
	sPCLastTick = sGyroLastTick = sRCLastTick = sPassLastTick = xTaskGetTickCount();
	for (;;)
	{
/*------------------小电脑数据和发射---------------------------*/
		if (xSemaphoreTake(UART_To_PCHandle, 1) == pdTRUE)
		{
			__disable_irq();
			for (uint8_t i = 0; i < 35; i++)  // 刷新小电脑数组 
			{
				sComputerData[i] = gComputer_Data[i];
			}
			__enable_irq();
			sPCLastTick = xTaskGetTickCount();
			sPC_Err = 0;
		}
		
		if (xTaskGetTickCount() >= sPCLastTick + 170)
		{
			if(sPC_Err < 60)
				sPC_Err++;	
			MX_USART2_UART_Init();  // 小电脑数据超时处理
			HAL_UARTEx_ReceiveToIdle_DMA(&huart2, gComputer_Data, 105);
		}

		for (uint8_t i = 0; i < 4; i++)  // 解析小电脑相关数据
			gComputer.u8[i] = sComputerData[i + 2];
		sComputer_HoopAng = gComputer.f;

		for (uint8_t i = 0; i < 4; i++)
			gComputer.u8[i] = sComputerData[i + 6];
		sComputer_X = gComputer.f;

		for (uint8_t i = 0; i < 4; i++)
			gComputer.u8[i] = sComputerData[i + 10];
		sComputer_Y = gComputer.f;

//		for (uint8_t i = 0; i < 4; i++)
//			gComputer.u8[i] = sComputerData[i + 14];
//		sHoop_Distance = gComputer.f;
		
		for (uint8_t i = 0; i < 4; i++)
			gComputer.u8[i] = sComputerData[i + 18];
		sPass_Ang = gComputer.f;
		
		for (uint8_t i = 0; i < 4; i++)
			gComputer.u8[i] = sComputerData[i + 22];
		sCamera_Distanse = gComputer.f;
		
		for (uint8_t i = 0; i < 4; i++)
			gComputer.u8[i] = sComputerData[i + 26];
		sComYaw_Ang = gComputer.f;
		
		gCamera_Radar_Sign = sComputerData[30];  // 雷达还是相机瞄准
		sComputer_ErrSign  = sComputerData[31];  // 小电脑是否需要重启
		
		sHoop_Distance = sqrt((cHoop_Y-sComputer_Y)*(cHoop_Y-sComputer_Y)
							+(cHoop_X-sComputer_X)*(cHoop_X-sComputer_X));
		
		if(sComputer_ErrSign == 1 || sPC_Err >= 55)
		{
			gErrState.pcErr = true;
		}
		else
		{
			gErrState.pcErr = false;
		}

		if ((sHoop_Distance > 1.65 && sComputer_X > 6)||gAim_Sign==2)  // 计算瞄准角度
			sCoordinates_HoopAng = My_RadToAng(atan2f(cHoop_Y - sComputer_Y, cHoop_X - sComputer_X))
									- sYaw_Ang;
		else
			sCoordinates_HoopAng = -sYaw_Ang;
		
		if(sComputer_ErrSign == 1 || fabs(sHoop_Distance) >=50 )
		{
			gAim_Sign = 1;
			sCoordinates_HoopAng = sComputer_HoopAng;
			sHoop_Distance = sCamera_Distanse;
		}
		if(gAim_Sign==1||gAim_Sign==2)  // 判断是相机还是雷达瞄准
		{
			if(gCamera_Radar_Sign == 1)
				gAim_Sign = 1;
			else
				gAim_Sign = 2;
		}		
		
		if (xSemaphoreTake(ShootHandle, 1) == pdTRUE)
		{
			xSemaphoreTake(ShootFinishHandle, 1); // 保证推射那边没被阻塞
			sShootCMD.Mode = eShoot_Push;		  // 发出指令让推射电机到达接球位置	
			
			if(sHoop_Distance<=5.25)
			{
				gShoot_V = 27.88821+5.59476*sHoop_Distance;
				if(sHoop_Distance>4.1)
					gShoot_V = 27.78821+5.59476*sHoop_Distance;	
			}
			else
			{
				gShoot_V = 12.46708+8.53758*sHoop_Distance;
			}
			
			if(gAim_Sign == 6)
			{
				if(sRobot_Distance<=5.25)
				{
					gShoot_V = 25.48821+5.59476*sRobot_Distance;
				}
				else
				{
					gShoot_V = 10.26708+8.53758*sRobot_Distance;
				}
			}
			
			if (gShoot_V >= 95)
				gShoot_V = 95;
			sShootCMD.Duty = gShoot_V;
			xQueueSend(Shoot_CMDHandle, &sShootCMD, portMAX_DELAY);
		}
/*----------------------两车通信---------------------------*/
		if (xSemaphoreTake(UART_To_RobotHandle, 1) == pdTRUE)
		{
			__disable_irq();
			for (uint8_t i = 0; i < 10; i++)
				sPassData[i] = gPass_Data[i];
			__enable_irq();
			sPassLastTick = xTaskGetTickCount();
			sRobot_Err = 0;
		}
		if (xTaskGetTickCount() >= sPassLastTick + 170) // 两车通信超时处理
		{
			if(sRobot_Err < 60)
				sRobot_Err++;
			MX_UART7_Init();
			HAL_UARTEx_ReceiveToIdle_DMA(&huart7, gPass_Data, 30);
		}
		if(sRobot_Err>55)
		{
			gErrState.twoCarErr = true;
		}else
		{
			gErrState.twoCarErr = false;
		}
		for (uint8_t i = 0; i < 4; i++)
			gPASS.u8[i] = sPassData[i+1];
		sRobot_X = gPASS.f;
		for (uint8_t i = 0; i < 4; i++)
			gPASS.u8[i] = sPassData[i+5];
		sRobot_Y = gPASS.f;
		sRobot_Distance = sqrt((sRobot_Y-sComputer_Y)*(sRobot_Y-sComputer_Y)
							+(sRobot_X-sComputer_X)*(sRobot_X-sComputer_X));
		sRobot_Ang = My_RadToAng(atan2f(sRobot_Y - sComputer_Y, sRobot_X - sComputer_X))
									- sYaw_Ang;
/*-------------------遥控和陀螺仪--------------------------*/		
		if (xSemaphoreTake(UART_To_RCHandle, 1) == pdTRUE)
		{
			__disable_irq();
			for (uint8_t i = 0; i < 14; i++) // 刷新遥控 
			{
				sRCRxData[i] = gRCRxData[i];
			}
			__enable_irq();
			sRCLastTick = xTaskGetTickCount();
		}

		gJoy_RC.u8[0] = sRCRxData[1];  // 解析左右摇杆
		gJoy_RC.u8[1] = sRCRxData[2];
		sVx_Receive_Middle = gJoy_RC.i16;
		gJoy_RC.u8[0] = sRCRxData[3];
		gJoy_RC.u8[1] = sRCRxData[4];
		sVy_Receive_Middle = gJoy_RC.i16;

		gJoy_RC.u8[0] = sRCRxData[5];
		gJoy_RC.u8[1] = sRCRxData[6];
		sVx_Receive = gJoy_RC.i16;
		gJoy_RC.u8[0] = sRCRxData[7];
		gJoy_RC.u8[1] = sRCRxData[8];
		sVy_Receive = gJoy_RC.i16;

		if (sRCRxData[9] != 0x00 && sRCRxData[9] != sLast_Button)  // 按钮
			xQueueSend(Button_BuffHandle, &sRCRxData[9], 1);
		sLast_Button = sRCRxData[9];
		
		if ((sRCRxData[10] != sLastAttackAndDefenseMode) && (sRCRxData[10] == 0xBB))
		{
			xQueueSend(Button_BuffHandle, &sRCRxData[10], 1);
			Sync_Log = 0;
		}
		sLastAttackAndDefenseMode = sRCRxData[10];

		if (xTaskGetTickCount() >= sRCLastTick + 170)  // 遥控超时处理
		{
			sVx_Receive_Middle = 0;  
			sVy_Receive_Middle = 0;
			sVx_Receive = 0;
			sVy_Receive = 0;
			MX_UART4_Init();
			HAL_UARTEx_ReceiveToIdle_DMA(&huart4, gRCRxData, 42);
		}

		if (xSemaphoreTake(UART_To_GyroHandle, 1) == pdTRUE)
		{
			__disable_irq();
			for (uint8_t i = 0; i < 14; i++)  // 刷新陀螺仪数组 
			{
				sGyroRxData[i] = gGyroRxData[i];
			}
			__enable_irq();
			sGyroLastTick = xTaskGetTickCount();
			sYaw_Err = 0;
		}

		if (xTaskGetTickCount() >= sGyroLastTick + 170) // 陀螺仪超时处理
		{
			if(sYaw_Err < 60)
				sYaw_Err++;
			MX_USART3_UART_Init();
			HAL_UARTEx_ReceiveToIdle_DMA(&huart3, gGyroRxData, 42);
		}
//		if(sComYaw_Ang != 0 && Sync_Log == 0)
//		{
//			sInlYaw_Ang = sComYaw_Ang;
//			Sync_Log = 1;
//		}
		if(sYaw_Err >= 55)
		{
			sYaw_Ang = sComYaw_Ang;
		}else
			sYaw_Ang = Gyro_Analysis(sGyroRxData,sInlYaw_Ang); // 解析陀螺仪数据
		
/*-----------------底盘移动和瞄准----------------------------*/
		switch (gChassis_Speed_Log)
		{
			case Normal:
				sYaw_Rad = My_AngToRad(-sYaw_Ang);  
				if (sVx_Receive_Middle == 0 && sVy_Receive_Middle == 0)
				{
					// 旋转坐标轴
					sVx = sVx_Receive * cos(sYaw_Rad) 
							- sVy_Receive * sin(sYaw_Rad);
					sVy = sVx_Receive * sin(sYaw_Rad) 
							+ sVy_Receive * cos(sYaw_Rad);
					sVz = sVx;
					sVx = -sVy * 2.7;
					sVy =  sVz * 2.7;
				}
				else
				{
					sVx = sVx_Receive_Middle * cos(sYaw_Rad) 
							- sVy_Receive_Middle * sin(sYaw_Rad);
					sVy = sVx_Receive_Middle * sin(sYaw_Rad) 
							+ sVy_Receive_Middle * cos(sYaw_Rad);
					sVz = sVx;
					sVx = -sVy * 1.7;
					sVy =  sVz * 1.7;
				}
				break;
			case Put:

				break;

			default:
				break;
		}
		
		sW = Calculate_W(-sComputer_HoopAng,sCoordinates_HoopAng,sYaw_Aim
							,sRobot_Ang,sYaw_Ang,gAim_Sign);  // 旋转速度计算
		if(gAim_Sign == 0 || gAim_Sign == 5)  
			Aim_LED = 0x0A;	 // 平常模式的灯光颜色
		if((gAim_Sign == 1||gAim_Sign == 2||gAim_Sign == 6)&&fabs(sW)>0.1)
		{
			Aim_LED = 0x0B;  // 正在瞄准的灯光
		}
		if(gAim_Sign == 6 && fabs(sW) < 0.1)
		{
			if(sAim_Time_Radar<15 && Aim_LED != 0x0E)			
				sAim_Time_Radar++;
			else
			{
				sAim_Time_Radar = 0;
				Aim_LED = 0x0E;  // 传球瞄准完毕
			}
		}
		if(gAim_Sign == 4)
		{	
			if(sBlock_Sign<20)					
				sBlock_Sign++;
			else
				Aim_LED = 0x0C;  // 相机瞄准完毕的灯光	
		}
		if(gAim_Sign==7)
		{
			if(sBlock_Sign < 20)					
				sBlock_Sign++;
			else
				Aim_LED = 0x0D;  // 雷达瞄准完毕
		}
		
		if(gAim_Sign == 2 && fabs(sCoordinates_HoopAng) < 0.25) // 保持雷达瞄好的角度
		{
			if(sAim_Time_Radar < 10)	
			{				
				sAim_Time_Radar++;
			}
			else
			{
				sAim_Time_Radar = 0;
				gAim_Sign = 7;
			}
		}
		if(gAim_Sign == 1 && fabs(sComputer_HoopAng)<0.26)  // 保持相机瞄好的角度 死区
		{	
			if(sAim_Time_Camera<35)			//改
				sAim_Time_Camera++;
			else
			{		
				sAim_Time_Camera = 0;
				gAim_Sign = 4;
			}
		}
		if((sVx!=0||sVx!=0)
			&&(gAim_Sign==1||gAim_Sign==2||gAim_Sign==4||gAim_Sign==7))  // 移动时取消保持瞄准角度
		{
			gAim_Sign = 0;
		}
		if(gAim_Sign == 0 || gAim_Sign != gAim_Sign_Last)
		{
			sBlock_Sign = 0;
			sAim_Time_Radar = 0;
			sAim_Time_Camera = 0;
		}
		gAim_Sign_Last = gAim_Sign;
		
		sVx = sVx*(1-fabs(sW*0.002));  // 代价函数
		sVy = sVy*(1-fabs(sW*0.002));
		
		Helm_Wheel_Control(sVx, sVy, sW, gAim_Sign, sBlock_Sign);  // 底盘移动函数
		
		snprintf(Pri,sizeof(Pri),"L:%f\r\n",sHoop_Distance);  // 打印距离和占空比
		HAL_UART_Transmit(&huart1, (uint8_t *)Pri, 13,0x01);
		snprintf(Pri,sizeof(Pri),"V:%f\r\n",gShoot_V);
		HAL_UART_Transmit(&huart1, (uint8_t *)Pri, 13,0x01);
		osDelay(2);
	}
}

/* 按键任务 ***************************************************************/

void buttonTask(void *argument)
{
	uint8_t Button = 0x00;
	static enum DribbleMode sDribble;
	static ShootCMD sShootCMD;

	for (;;)
	{
		if (xQueueReceive(Button_BuffHandle, &Button, portMAX_DELAY))
		{
			switch (Button)
			{
			case 0x01:
				sDribble = eDribble_One;  // 将运球升起
				xQueueSend(Dribble_CMDHandle, &sDribble, portMAX_DELAY);
				break;

			case 0x02:
				sDribble = eDribble_Two;  // 开始运球
				xQueueSend(Dribble_CMDHandle, &sDribble, portMAX_DELAY);
				break;

			case 0x03:
				sDribble = eDribble_HandGuard;  // 挡手
				xQueueSend(Dribble_CMDHandle, &sDribble, portMAX_DELAY);
				break;

			case 0x04:
				gAim_Sign = 2;  // 瞄准
				break;

			case 0x05:
				sShootCMD.Mode = eShoot_Bottom; // 发出指令让推射电机到底部
				xQueueSend(Shoot_CMDHandle, &sShootCMD, portMAX_DELAY);
				break;

			case 0x06:
				xSemaphoreGive(ShootHandle);  // 发射
				break;

			case 0x07:
				if(gAim_Sign != 6)
					gAim_Sign = 6;  // 传球瞄准
				else
					gAim_Sign = 0;  // 取消传球瞄准
				break;

			case 0x08:
				gShoot_V += 0.25;
				break;

			case 0x09:
				gShoot_V -= 0.25;
				break;

			case 0xBB:
				sDribble = eDribble_Defense;  // 防守模式
				xQueueSend(Dribble_CMDHandle, &sDribble, portMAX_DELAY);
				break;

			default:
				break;
			}
		}
	}
}
